#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/float64.hpp"

// 目标角度/位置
float target_j1 = 0.0, target_j2 = 0.0, target_j3 = 0.0;
// 实时角度/行程
float rt_j1 = 0.0, rt_j2 = 0.0, rt_j3 = 0.0;
//
float j1_cbdata = 0.0, j2_cbdata = 0.0, j3_cbdata = 0.0;
//
float j1_icbdata = 0.0, j2_icbdata = 0.0, j3_icbdata = 0.0;

// 3.自定义节点类；
class Arm_Control : public rclcpp::Node
{
public:
    Arm_Control() : Node("arm_control_node_cpp")
    {
        // 创建发布方
        // --向逆解程序发布坐标
        this->tar_x = this->create_publisher<std_msgs::msg::Float64>("x_goat", 5);
        this->tar_y = this->create_publisher<std_msgs::msg::Float64>("y_goat", 5);
        this->tar_z = this->create_publisher<std_msgs::msg::Float64>("z_goat", 5);
        // 创建订阅方
        // 订阅来自编码器和 ADC的数据
        this->joint1_sub = this->create_subscription<std_msgs::msg::Int32>("joint1_p_", 5, std::bind(&Arm_Control::j1_cb, this, std::placeholders::_1));
        this->joint2_sub = this->create_subscription<std_msgs::msg::Float64>("joint2_p_", 5, std::bind(&Arm_Control::j2_cb, this, std::placeholders::_1));
        this->joint3_sub = this->create_subscription<std_msgs::msg::Float64>("joint3_p_", 5, std::bind(&Arm_Control::j3_cb, this, std::placeholders::_1));
        // 订阅逆解求得的数据，角度、行程
        this->j1_sub = this->create_subscription<std_msgs::msg::Float64>("joint1_gp_", 5, std::bind(&Arm_Control::j1_icb, this, std::placeholders::_1));
        this->j2_sub = this->create_subscription<std_msgs::msg::Float64>("joint2_gp_", 5, std::bind(&Arm_Control::j2_icb, this, std::placeholders::_1));
        this->j3_sub = this->create_subscription<std_msgs::msg::Float64>("joint3_gp_", 5, std::bind(&Arm_Control::j3_icb, this, std::placeholders::_1));

        // 添加定时器（ms周期，按需调整）
        check_timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&Arm_Control::check_target_reached, this));
        check_timer_->cancel(); // 默认不启动
    }

    void go_target(const std::vector<std::tuple<float, float, float>>& targets)
    {
        target_sequence = targets;
        process_next_target();
    }

private:
    // 目标序列
    std::vector<std::tuple<float, float, float>> target_sequence;

    bool j1cb_updated = false;
    bool j2cb_updated = false;
    bool j3cb_updated = false;
    bool j1icb_updated = false;
    bool j2icb_updated = false;
    bool j3icb_updated = false;
    rclcpp::TimerBase::SharedPtr check_timer_;

    // 目标位置发布
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr tar_x;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr tar_y;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr tar_z;

    // 一号编码器，二三号ADC的数据
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr joint1_sub;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint2_sub;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint3_sub;
    // 逆解求得的数据，角度、行程
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr j1_sub;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr j2_sub;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr j3_sub;

    void process_next_target()
    {
        if (!target_sequence.empty())
        {
            auto target = target_sequence.front();
            float x = std::get<0>(target);
            float y = std::get<1>(target);
            float z = std::get<2>(target);

            // 发布目标
            auto tarv_x = std_msgs::msg::Float64();
            auto tarv_y = std_msgs::msg::Float64();
            auto tarv_z = std_msgs::msg::Float64();
            tarv_x.data = x;
            tarv_y.data = y;
            tarv_z.data = z;
            tar_x->publish(tarv_x);
            tar_y->publish(tarv_y);
            tar_z->publish(tarv_z);

            // 重置标志并启动定时器
            reset_flags();
            check_timer_->reset();

            // 移除已处理的目标
            target_sequence.erase(target_sequence.begin());
        }
    }

    void j1_cb(const std_msgs::msg::Int32::SharedPtr j1_msg)
    {
        j1_cbdata = float(j1_msg->data);
        j1cb_updated = true;
    }
    void j2_cb(const std_msgs::msg::Float64::SharedPtr j2_msg)
    {
        j2_cbdata = j2_msg->data;
        j2cb_updated = true;
    }
    void j3_cb(const std_msgs::msg::Float64::SharedPtr j3_msg)
    {
        j3_cbdata = j3_msg->data;
        j3cb_updated = true;
    }
    //
    void j1_icb(const std_msgs::msg::Float64::SharedPtr j1_imsg)
    {
        j1_icbdata = j1_imsg->data;
        j1icb_updated = true;
    }
    void j2_icb(const std_msgs::msg::Float64::SharedPtr j2_imsg)
    {
        j2_icbdata = j2_imsg->data;
        j2icb_updated = true;
    }
    void j3_icb(const std_msgs::msg::Float64::SharedPtr j3_imsg)
    {
        j3_icbdata = j3_imsg->data;
        j3icb_updated = true;
    }

    void check_target_reached()
    {

        // 只有所有数据更新后才进行检查
        if (!(j1cb_updated && j2cb_updated && j3cb_updated &&
              j1icb_updated && j2icb_updated && j3icb_updated))
        {
            RCLCPP_DEBUG(this->get_logger(), "等待数据更新...");
            return;
        }

        // 计算误差
        const float tolerance_1 = 2;
        const float tolerance_2 = 4;
        const float tolerance_3 = 4;

        float error_j1 = std::abs(j1_cbdata - j1_icbdata);
        float error_j2 = std::abs(j2_cbdata - j2_icbdata);
        float error_j3 = std::abs(j3_cbdata - j3_icbdata);

        if (error_j1 <= tolerance_1 && error_j2 <= tolerance_2 && error_j3 <= tolerance_3)
        {
            RCLCPP_INFO(this->get_logger(), "目标已到达!");
            check_timer_->cancel(); // 停止定时器
            process_next_target(); // 处理下一个目标
        }
        else
        {
            RCLCPP_INFO_THROTTLE( // 节流输出避免刷屏
                this->get_logger(), *this->get_clock(), 1000,
                "未到达目标，当前误差: j1=%.3f, j2=%.3f, j3=%.3f",
                error_j1, error_j2, error_j3);
        }
        // 重置标志等待下次更新
        reset_flags();
    }
    void reset_flags()
    {
        j1cb_updated = false;
        j2cb_updated = false;
        j3cb_updated = false;
        j1icb_updated = false;
        j2icb_updated = false;
        j3icb_updated = false;
    }
};

int main(int argc, char const *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<Arm_Control>();
    
    // 创建目标序列
    std::vector<std::tuple<float, float, float>> targets = {
        {1.0, 2.0, 3.0}, // 第一个目标
        {4.0, 5.0, 6.0}, // 第二个目标
        {7.0, 8.0, 9.0}  // 第三个目标
    };

    node->go_target(targets); // 启动目标序列

    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}